package centerberg.engine;

import java.awt.Point;
import java.awt.geom.Area;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Rectangle2D;
import java.util.Collection;
import java.util.HashSet;
import java.util.LinkedList;

/**
 * Simulates physics. Updates position based on forces applied to an object
 * including gravity and any others.
 * 
 * <p>Notifies objects that they have collided. The order in which any object A
 * and B gets notified that they have collided is arbitrary.
 * 
 * <p>The unit for force is <em>pixels/(10000ms^2)</em>.
 * 
 * @author Andreas Svanberg
 */
public class PhysicsEngine
{
    public static final Vector DEFAULT_GRAVITY = new Vector(0, 9.89);
    public static final double GRAVITY_CONVERSION_CONSTANT = 1/10000.0;
    
    private Vector                    gravity       = DEFAULT_GRAVITY;
    private Collection<RigidBody>     bodies        = new LinkedList<RigidBody>();
    private Collection<PhysicsObject> physicalWorld = new LinkedList<PhysicsObject>();
    private CollisionSystem           cs            = new CollisionSystem();
    
    /**
     * Runs a simulation of the physical world. Updates forces,
     * velocities, and position. Checks and notifies collisions.
     * 
     * @param elapsed the amount of milliseconds since last simulation
     */
    public void runSimulation(double elapsed)
    {
        if (elapsed == 0)
            return;
        
        for (RigidBody body : bodies) // update physics loop
        {
            if (body.getMass() > 0)
            {
                // add gravity
                body.addForce(gravity.multiply(body.getMass()).multiply(GRAVITY_CONVERSION_CONSTANT));
                
                Vector accel = body.getForce().divide(body.getMass()).multiply(elapsed);
                body.addVelocity(accel);
                double dx = body.getVelocity().getX() * elapsed;
                double dy = body.getVelocity().getY() * elapsed;
                move(body, dx, dy);
                
                // remove remaining force, speed does not decay, yet
                // TODO: add friction
                body.setForce(new Vector(0, 0));
            }
        }
        
        for (RigidBody body : bodies) // collision loop
        {
            Collection<CollisionResult> collisions = cs.testCollisions(body);
            for (CollisionResult result : collisions)
            {
                // collision notification
                body.collided(result.otherObject);
                result.otherObject.collided(body); // tell the other object as well since we uncollide the object that objects collision check wont trigger
                
                if (result.otherObject instanceof RigidBody)
                {
                    RigidBody otherBody = (RigidBody)result.otherObject;
                    
                    if (body.getPhysicalRepresentation().getShape() instanceof Ellipse2D)
                    {
                        if (result.otherObject.getPhysicalRepresentation().getShape() instanceof Ellipse2D)
                        {
                            doCircleCircleCollision(body, otherBody, result);
                        }
                        else
                        {
                            doSimpleCollision(body, otherBody, result);
                        }
                    }
                    else
                    {
                        if (result.otherObject.getPhysicalRepresentation().getShape() instanceof Ellipse2D)
                        {
                            doSimpleCollision(body, otherBody, result);
                        }
                        else
                        {
                            doSimpleCollision(body, otherBody, result);
                        }
                    }
                }
                else if (result.otherObject instanceof Solid)
                {
                    // uncollide it, only in directions it was actually moving
                    if (body.getVelocity().getX() != 0)
                        move(body, result.normal.getX(), 0);
                    if (body.getVelocity().getY() != 0)
                        move(body, 0, result.normal.getY());
                    
                    result.normal.normalize();
                    Vector tangent = new Vector(result.normal.getY(), -result.normal.getX());
                    Vector normVelocity = result.normal.multiply(Vector.dot(body.getVelocity(), result.normal));
                    Vector tanVelocity = tangent.multiply(Vector.dot(body.getVelocity(), tangent));
                    Vector newVelocity = tanVelocity.add(normVelocity.multiply(-1).multiply(body.getBounciness()));
                    
                    body.setVelocity(newVelocity);
                }
            }
        }
    }
    
    /**
     * Adds an object to the physical world.
     * 
     * @param obj the object to add
     */
    public void addToWorld(PhysicsObject obj)
    {
        physicalWorld.add(obj);
        if (obj instanceof RigidBody)
            bodies.add( (RigidBody) obj);
    }
    
    /**
     * Removes an object from the physical world.
     * 
     * @param obj the object to remove
     */
    public void removeFromWorld(PhysicsObject obj)
    {
        physicalWorld.remove(obj);
        if (obj instanceof RigidBody)
            bodies.remove((RigidBody)obj);
    }
    
    /**
     * Clears the world from all objects.
     */
    public void clearWorld()
    {
        physicalWorld.clear();
        bodies.clear();
    }
    
    private void doCircleCircleCollision(RigidBody o1, RigidBody o2, CollisionResult result)
    {
        Vector normal = o1.getCenter().subtract(o2.getCenter());
        
        double distance = normal.length();
        double o1Radius = o1.getPhysicalRepresentation().getShape().getBounds2D().getHeight() / 2; // only circles supported
        double o2Radius = o2.getPhysicalRepresentation().getShape().getBounds2D().getHeight() / 2;
        
        normal.normalize();
        
        Vector tangent = new Vector(normal.getY(), -normal.getX());
        Vector uncollide = normal.multiply(o1Radius + o2Radius - distance);
        
        double o1Mass = o1.getMass();
        double o2Mass = o2.getMass();
        move(o1, uncollide.multiply(o1Mass / (o1Mass + o2Mass)));
        move(o2, uncollide.multiply(-o2Mass / (o1Mass + o2Mass)));
        
        doVelocityUpdate(o1, o2, normal, tangent);
    }

    
    private void doSimpleCollision(RigidBody o1, RigidBody o2, CollisionResult result)
    {
        move(o1, result.normal);
        result.normal.normalize();
        
        Vector normal = result.normal;
        Vector tangent = new Vector(result.normal.getY(), -normal.getX());
        
        doVelocityUpdate(o1, o2, normal, tangent);
    }
    
    private void doVelocityUpdate(RigidBody o1, RigidBody o2, Vector normal, Vector tangent)
    {
        double o1Mass = o1.getMass();
        double o2Mass = o2.getMass();
        double totalMass = o1Mass + o2Mass;
        
        Vector o1Velocity = o1.getVelocity();
        Vector o2Velocity = o2.getVelocity();
        
        Vector o1NormalVelocity = normal.multiply(Vector.dot(o1Velocity, normal));
        Vector o1TangentVelocity = tangent.multiply(Vector.dot(o1Velocity, tangent));
        
        Vector o2NormalVelocity = normal.multiply(Vector.dot(o2Velocity, normal));
        Vector o2TangentVelocity = tangent.multiply(Vector.dot(o2Velocity, tangent));
        
        o1.setVelocity(
            o1TangentVelocity.add(
                normal.multiply( (o1Mass - o2Mass) / totalMass * o1NormalVelocity.length() + 2 * o2Mass / totalMass * o2NormalVelocity.length() )
                      
            )
        );
        o2.setVelocity(
            o2TangentVelocity.add(
                normal.multiply( (o2Mass - o1Mass) / totalMass * o2NormalVelocity.length() + 2 * o1Mass / totalMass * o1NormalVelocity.length() )
                      
            )
        );
    }
    
    /**
     * Moves the specified body by the specified vector.
     * 
     * @param body the body to move
     * @param v    the vector to move it by
     */
    private void move(RigidBody body, Vector v)
    {
        move(body, v.getX(), v.getY());
    }
    
    /**
     * Moves the specified body the specified distance in x- and y-space.
     * 
     * @param body the body to move
     * @param dx   the distance to move in the x-axis
     * @param dy   the distance to move in the y-axis
     */
    private void move(RigidBody body, double dx, double dy)
    {
        body.getPhysicalRepresentation().translate(dx, dy);
        body.translated(dx, dy);
    }
    
    /**
     * Sets the gravity for this physical simulator.
     * 
     * @param gravity the new gravity
     * @see PhysicsEngine#DEFAULT_GRAVITY
     * @see PhysicsEngine#GRAVITY_CONVERSION_CONSTANT
     */
    public void setGravity(double gravity)
    {
        this.gravity = new Vector(0.0, gravity);
    }
    
    /**
     * The system handling collision checking.
     * Currently it only supports rectangles, rhomboids (only y-sheared)
     * and ellipses.
     */
    private class CollisionSystem
    {
        public Collection<CollisionResult> testCollisions(PhysicsObject obj)
        {
            Rectangle2D objBounds = obj.getPhysicalRepresentation().getArea().getBounds2D();
            Collection<CollisionResult> collisions = new LinkedList<CollisionResult>();
            
            for (PhysicsObject other : physicalWorld)
            {
                if (other == obj)
                    continue;
                
                Area intersection = (Area)obj.getPhysicalRepresentation().getArea().clone();
                PhysicalRepresentation otherPhys = other.getPhysicalRepresentation();
                intersection.intersect(otherPhys.getArea());
                
                if (!intersection.isEmpty())
                {
                    Rectangle2D intersectBounds = intersection.getBounds2D();
                    CollisionResult result = new CollisionResult();
                    
                    result.otherObject = other;
                    
                    result.collisionPoint = new Point();
                    result.collisionPoint.setLocation(intersectBounds.getCenterX(), intersectBounds.getCenterY());
                    
                    if (otherPhys.getShape() instanceof Rectangle2D)
                    {
                        Rectangle2D rect = (Rectangle2D)otherPhys.getArea().getBounds2D();
                        
                        if (otherPhys.getTransform().getShearY() == 0)
                        {
                            if (intersectBounds.getWidth() > intersectBounds.getHeight())
                            {
                                if (rect.getMinY() < intersectBounds.getMinY())
                                    result.normal = new Vector(0.0, 1.0);
                                else
                                    result.normal = new Vector(0.0, -1.0);
                                result.normal = result.normal.multiply(intersectBounds.getHeight());
                            }
                            else
                            {
                                if (rect.getMinX() < intersectBounds.getMinX())
                                    result.normal = new Vector(1.0, 0.0);
                                else
                                    result.normal = new Vector(-1.0, 0.0);
                                result.normal = result.normal.multiply(intersectBounds.getWidth());
                            }
                        }
                        else
                        {
                            double shear = otherPhys.getTransform().getShearY();
                            double dx = rect.getMaxX() - rect.getMinX();
                            double dy = rect.getY() - (rect.getY() - rect.getMaxX() * otherPhys.getTransform().getShearY());
                            
                            Area otherArea = otherPhys.getArea();
                            Rectangle2D otherShape = (Rectangle2D)otherPhys.getShape();
                            
                            if (otherArea.contains(objBounds.getMinX(), objBounds.getMaxY()))
                            {
                                // bottom left corner
                                if (shear < 0)
                                    result.normal = new Vector(1, 0);
                                else
                                    result.normal = new Vector(dy, -dx);
                            }
                            else if(otherArea.contains(objBounds.getMaxX(), objBounds.getMaxY()))
                            {
                                // bottom right corner
                                if (shear < 0)
                                    result.normal = new Vector(dy, -dx);
                                else
                                    result.normal = new Vector(-1, 0);
                            }
                            else if (otherArea.contains(objBounds.getMinX(), objBounds.getMinY()))
                            {
                                // top left corner
                                if (shear < 0)
                                    result.normal = new Vector(-dy,  dx);
                                else
                                    result.normal = new Vector(1, 0);
                            }
                            else if(otherArea.contains(objBounds.getMaxX(), objBounds.getMinY()))
                            {
                                // top right corner
                                if (shear < 0)
                                    result.normal = new Vector(-1, 0);
                                else
                                    result.normal = new Vector(-dy, dx);
                            }
                            else if (objBounds.getMinX() < rect.getMinX())
                            {
                                // collision from left
                                if (shear > 0 && objBounds.getMaxY() < rect.getMinY() + otherShape.getHeight())
                                    result.normal = new Vector(0, -1);
                                else if (shear < 0 && objBounds.getMinY() > rect.getMaxY() - otherShape.getHeight())
                                    result.normal = new Vector(0, 1);
                                else
                                    result.normal = new Vector(-1, 0);
                            }
                            else
                            {
                                // collision from right
                                if (shear < 0 && objBounds.getMaxY() < rect.getMinY() + otherShape.getHeight())
                                    result.normal = new Vector(0, -1);
                                else if (shear > 0 && objBounds.getMinY() > rect.getMaxY() - otherShape.getHeight())
                                    result.normal = new Vector(0, 1);
                                else
                                    result.normal = new Vector(1, 0);
                            }
                            
                            result.normal.normalize();
                        }
                    }
                    else if (otherPhys.getShape() instanceof Ellipse2D)
                    {
                        Rectangle2D circleBounds = otherPhys.getArea().getBounds2D();
                        Vector c = new Vector(circleBounds.getCenterX(), circleBounds.getCenterY());
                        Vector v = new Vector(result.collisionPoint.getX(), result.collisionPoint.getY());
                        v = v.subtract(c);
                        v.normalize();
                        v.set(0, v.getX() * intersectBounds.getWidth());
                        v.set(1, v.getY() * intersectBounds.getHeight());
                        
                        result.normal = v;
                    }
                    
                    collisions.add(result);
                }
            }
            
            return collisions;
        }
    }
}
